#include "pathological.h"


#define RAN_LAG 1024


uint32_t ranlag[RAN_LAG], ranc, ranlagi;


void ran_init()
{
	int i;
	srand(time(NULL));
	ranc = rand();
	ranlagi = rand() % RAN_LAG;
	ranlag[0] = rand();
	for(i = 1; i < RAN_LAG; i++)
		ranlag[i] = 2871516421LL*ranlag[i-1] + 654729075;
}

inline float ran()
{
#define RAN_FACTOR 3636507990LL
#ifdef X86_ASM
	float r;
	asm("mull %3;"
		"add %4, %%eax;"
		"mov %%eax, %0;"
		"adc $0, %%edx;"
		"mov %%edx, %1;"
		"cmp $0, %%eax;"
		"jz .done_%=;"
			"shl $9, %%eax;"
			"bsr %%eax, %%ecx;"
			"neg %%cl;"
			"add $32, %%cl;"
			"shl %%cl, %%eax;"
			"mov $0x7F, %%al;"
			"sub %%cl, %%al;"
			"ror $9, %%eax;"
	".done_%=:;"
		:"=m"(ranlag[ranlagi]),"=m"(ranc),"=a"(r)
		:"m"(ranlag[ranlagi]),"m"(ranc),"r"(RAN_FACTOR)
		:"ecx","edx");
	if(++ranlagi >= RAN_LAG) ranlagi = 0;
	return r;
#else
	unsigned long long m = RAN_FACTOR*ranlag[ranlagi] + ranc;
	float r = (ranlag[ranlagi] = (m & 0xFFFFFFFF))*(1.0f/0xFFFFFFFF);
	if(++ranlagi >= RAN_LAG) ranlagi = 0;
	ranc = m / 0x100000000LL;
	return r;
#endif
}

inline double dot(vector_t *v1, vector_t *v2)
{
	return (double)v1->x*v2->x + (double)v1->y*v2->y + (double)v1->z*v2->z;
}

// *result = *v1 - *v2
inline vector_t *vec_sub(vector_t *v1, vector_t *v2, vector_t *result)
{
	result->x = v1->x - v2->x;
	result->y = v1->y - v2->y;
	result->z = v1->z - v2->z;
	return result;
}

inline vector_t *vec_scale(vector_t *v, double s)
{
	v->x *= s, v->y *= s, v->z *= s;
	return v;
}
/*
inline double fast_atan(double x) // x >= 0
{
	// Constants calulated by GNUPlot
	const double a = -0.251399293945138, b = 1.04494642919232;
	if(x < 1) return (a*x + b)*x;
	else return PI/2 - (a/x + b)/x;
}
*/
inline void rref_3x4(double **m)
{
#define SWAP_X4_ROWS(r1, r2) \
	{double *trow; trow = (r1); (r1) = (r2); (r2) = trow;}

#define ADD_X4_ROW(r1, r2, scale) \
	{ \
		double s = (scale); \
		(r1)[0] += s*(r2)[0], (r1)[1] += s*(r2)[1], \
		(r1)[2] += s*(r2)[2], (r1)[3] += s*(r2)[3]; \
	}

#define SCALE_X4_ROW(r, scale) \
	{double s = (scale); (r)[0] *= s, (r)[1] *= s, (r)[2] *= s, (r)[3] *= s;}

	if(m[0][0] == 0)
	{
		if(m[1][0] == 0) SWAP_X4_ROWS(m[0],m[2])
		else SWAP_X4_ROWS(m[0],m[1])
	}
	SCALE_X4_ROW(m[0],1/m[0][0]);
	ADD_X4_ROW(m[1],m[0],-m[1][0]);
	ADD_X4_ROW(m[2],m[0],-m[2][0]);
	if(m[1][1] == 0) SWAP_X4_ROWS(m[1],m[2]);
	SCALE_X4_ROW(m[1],1/m[1][1]);
	ADD_X4_ROW(m[0],m[1],-m[0][1]);
	ADD_X4_ROW(m[2],m[1],-m[2][1]);
	if(m[2][2] == 0) return;
	SCALE_X4_ROW(m[2],1/m[2][2]);
	ADD_X4_ROW(m[0],m[2],-m[0][2]);
	ADD_X4_ROW(m[1],m[2],-m[1][2]);
}

